// Turn on 2 motors only and set PWM speeds


//Define variables
int LED = 16;        //LED on pin 6
int INA1 = 2;      //h-bridge INA1 on pin 12
int INA2 = 3;      //h-bridge INA2 on pin 13
int PWMA = 0;       //h-bridge PWMA on pin 2
int INB1 = 4;      //h-bridge INB1 on pin 14
int INB2 = 5;      //h-bridge INB2 on pin 15
int PWMB = 1;       //h-bridge PWMB pin 3
int velocity = 50; //set motor speed between 0-255
int delayTime = 1;  //millisecond
int distance = 0;   //initialize variable to store distance later

void setup() {
  Serial.begin(115200);
  //set microprocessor pin modes
  pinMode(LED, OUTPUT);
  pinMode(INA1, OUTPUT);
  pinMode(INA2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(INB1, OUTPUT);
  pinMode(INB2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  //turn LED on
  digitalWrite(LED, HIGH); 
  Serial.println("Setup OK");
}

void loop() {
  //Motor A has green wire, Motor B has blue wire
  
  //Motor A - (CW) away from me
  digitalWrite(INA1,HIGH); 
  digitalWrite(INA2,LOW);
  analogWrite(PWMA,velocity); 
  delay(5000);
  //Motor A - break
  digitalWrite(INA1,HIGH); 
  digitalWrite(INA2,HIGH);
  analogWrite(PWMA,velocity); 
  delay(5000);
  //Motor A - (CCW) towards me
  digitalWrite(INA1,LOW); 
  digitalWrite(INA2,HIGH);
  analogWrite(PWMA,velocity); 
  delay(5000);
  //Motor A - coast
  digitalWrite(INA1,LOW); 
  digitalWrite(INA2,LOW);
  analogWrite(PWMA,velocity); 
  delay(5000);
  
  //Motor B - (CCW) away from me
  digitalWrite(INB1,HIGH); 
  digitalWrite(INB2,LOW); 
  analogWrite(PWMB,velocity);
  delay(5000);
  //Motor B - break
  digitalWrite(INB1,HIGH); 
  digitalWrite(INB2,HIGH); 
  analogWrite(PWMB,velocity);
  delay(5000);
  //Motor B - (CW) towards me
  digitalWrite(INB1,LOW); 
  digitalWrite(INB2,HIGH); 
  analogWrite(PWMB,velocity);
  delay(5000);
  //Motor B - coast
  digitalWrite(INB1,LOW); 
  digitalWrite(INB2,LOW); 
  analogWrite(PWMB,velocity);
  delay(5000);
  //Serial.println("Loop OK");
 }